﻿/*
 * ©2009-2010 XELF
 * http://xelf.info
 */

#if XNA || SILVERLIGHT || WINDOWS_PHONE
using Microsoft.Xna.Framework;
#endif

namespace XELF.Framework {
#if XNA || SILVERLIGHT || WINDOWS_PHONE
	public static partial class MatrixHelper {
#else
	public partial struct Matrix {
#endif
		/// <summary>
		/// デュアルクォータニオンから行列
		/// </summary>
		/// <param name="value"></param>
		/// <param name="result"></param>
		/// <returns></returns>
		public static Matrix CreateFromDualQuaternion(ref DualQuaternion value, out Matrix result) {
			var ll = value.Real.LengthSquared();
			var r = 1 / ll;
			float w = value.Real.W, x = value.Real.X, y = value.Real.Y, z = value.Real.Z;
			float w2 = w + w, x2 = x + x, y2 = y + y, z2 = z + z;
			float ww = w * w2, xx = x * x2, yy = y * y2, zz = z * z2;
			float wx = w * x2, wy = w * y2, wz = w * z2, xy = x * y2, xz = x * z2, yz = y * z2;
			float a = value.Dual.W, b = value.Dual.X, c = value.Dual.Y, d = value.Dual.Z;

			result = new Matrix();
			result.M11 = ll - yy - zz;
			result.M12 = xy + wz;
			result.M13 = xz - wy;
			result.M21 = xy - wz;
			result.M22 = ll - xx - zz;
			result.M23 = yz + wx;
			result.M31 = xz + wy;
			result.M32 = yz - wx;
			result.M33 = ll - xx - yy;
			result.M41 = -a * x2 + w2 * b - c * z2 + y2 * d;
			result.M42 = -a * y2 + b * z2 - x2 * d + w2 * c;
			result.M43 = -a * z2 + x2 * c + w2 * d - b * y2;
			result.M14 = 0;
			result.M24 = 0;
			result.M34 = 0;
			result.M44 = ll;

			Matrix.Multiply(ref result, r, out result);
			return result;
		}
	}

}